package roborally.model;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.Iterator;
import java.util.List;
import java.util.Random;
import java.util.Set;

import roborally.model.Energy.Unit;
import roborally.script.*;

import dijkstra.BoardGraphConverter;

import be.kuleuven.cs.som.annotate.*;
import be.kuleuven.cs.som.taglet.*;
import exception.IllegalPositionException;
import exception.IsTerminatedException;

/**
 * A Class of robots with a certain position, orientation and energy able to move and rotate. Movements use energy, energy can be recharged.
 * A robot can pick up elements from the board and use or drop them again. Added elements increase the robot's load, the heavier the load, the more energy is needed to move.
 * 
 * @invar The position must be a valid position
 * 		  | if this.getBoard() != null
 * 		  | then getBoard().isValidposition(this.getPosition())
 * 
 * @invar The orientation must be a valid orientation
 * 		  | this.getOrientation() != null && Orientation.values().contains(this.getOrientation())
 * 
 * @invar The Energy must be a valid Energy
 * 		  | this.canHaveAsEnergy(this.getEnergy().getValueIn(Energy.Unit.WS))
 * 
 * @invar The maximum Energy must be valid
 * 		  | this.canHaveAsCurrentMaxEnergy(this.getMaxEnergy())
 * 
 * @author Mathieu REYMOND
 * @author Joren VERSPEURT
 * 
 * @version 2.0
 *
 */
public class Robot extends Element implements IEnergyHolder {

	/**
	 * Create a robot with a given orientation, energy and maximum amount of energy.
	 * 
	 * @param orientation The orientation of the new robot
	 * @param energyAmount The amount of energy this robot will have
	 * @param maxEnergyAmount The maximum energy amount of the robot
	 * 
	 * @effect Call the most extensive constructor of the robot and give null as board and position
	 * 	       | this(null, null, orientation, energyAmount, maxEnergyAmount)
	 * 
	 * @throws IllegalPositionException
	 * 		   The given position is not valid
	 */
	public Robot(Orientation orientation, double energyAmount, double maxEnergyAmount) throws IllegalArgumentException, IllegalPositionException {
		
		this(null, null, orientation, energyAmount, maxEnergyAmount);
	}
	
	/**
	 * Create a robot with a given board, position, orientation, energy and maximum amount of energy in Ws.
	 * 
	 * @param board The board the robot will have
	 * @param position The position the robot will have
	 * @param orientation The orientation of the new robot
	 * @param energyAmount The amount of energy this robot will have in Ws
	 * @param maxEnergyAmount The maximum energy amount of the robot in Ws
	 * 
	 * @throws IllegalArgumentException
	 * @throws IllegalPositionException
	 * 
	 * @pre The given energy must be a valid amount of energy in Ws
	 * 	  	| new.canHaveAsEnergy(energyAmount)
	 * 
	 * @pre The given maximum amount of energy must be a valid energy and must be greater than or equal to the given amount of energy in Ws
	 * 		| energyAmount <= new.getMaxEnergy()
	 * 		| canHaveAsCurrentMaxEnergy(maxEnergyAmount)
	 * 
	 * @post The maximum amount of energy in Ws for this robot is maxEnergyAmount
	 * 		 | new.getMaxEnergy() == maxEnergyAmount
	 * 
	 * @effect Set the element's board and position
	 * 		   | this.setBoardPosition(board, position)
	 * 
	 * @effect Set the robot's orientation to the given orientation
	 * 		   | this.setOrientation(orientation)
	 * 
	 * @effect Set the robot's energy to the given energy in Ws
	 * 		   | this.setEnergy(new Energy(energyAmount))
	 */
	public Robot(Board board, Position position, Orientation orientation, double energyAmount, double maxEnergyAmount) throws IllegalArgumentException, IllegalPositionException {
		
		setBoardPosition(board, position);
		setOrientation(orientation);
		
		currentMaxEnergy = new Energy(maxEnergyAmount);
		MAX_ENERGY = new Energy(maxEnergyAmount);
		
		setEnergy(new Energy(energyAmount));
		items = new ArrayList<Item>();
		
		
		
	}
	
	/**
	 * Gives the robot's current orientation
	 * 
	 * @return the robot's current orientation
	 */
	@Basic
	public Orientation getOrientation() {
		return orientation;
	}

	/**
	 * Set the robot's orientation to the given orientation. If it is not a valid orientation, set the orientation to Orientation.UP
	 * 
	 * @param newOrientation the given orientation
	 */
	@Basic @Raw
	public void setOrientation(Orientation newOrientation) { //total way
		
		if(newOrientation != null) {
			orientation = newOrientation; //ENUM always valid
		}
		else {
			orientation = Orientation.UP;
		}
	}

	/**
	 * Turn the robot in the given direction
	 * 
	 * @param direction The given direction
	 * 
	 * @throws IsTerminatedException
	 * 		   Cannot turn a terminated robot
	 * 
	 * @pre The robot has enough energy to turn
	 * 		| this.getEnergy().getValueIn(Energy.Unit.WS).getValueIn(Energy.Unit.WS) >= getEnergyRequiredToTurn()
	 * 
	 * @post The robot's orientation turned 90 degrees in the given direction
	 * 		 | setOrientation(getOrientation().turn90(direction)
	 * 
	 * @effect The robot's energy is reduced by the energy amount required to turn
	 * 		 | setEnergy(getEnergy().getValueIn(Energy.Unit.WS).getValueIn(Energy.Unit.WS)-getEnergyRequiredToTurn())
	 */
	public void turn(Direction direction) throws IsTerminatedException {
		
		if(this.isTerminated()) throw new IsTerminatedException("Terminated Robots cannot turn");
		setOrientation(getOrientation().turn90(direction));
		setEnergy(new Energy(getEnergy().getValueIn(Energy.Unit.WS)-getEnergyRequiredToTurn().getValueIn(Energy.Unit.WS)));
	}

	private Orientation orientation;

	/**
	 * Checks if <code>energy</code> is a valid Energy.
	 * 
	 * @param energy The amount of energy to be checked
	 * @return True if valid, false otherwise.
	 * 		   | (energy >=0 && energy <= getMaxEnergy())
	 */
	public boolean canHaveAsEnergy(Energy energy) {
		
		return (energy.getValueIn(Energy.Unit.WS) >=0 && energy.compareTo(getCurrentMaxEnergy()) <= 0);
	}
	
	/**
	 * Checks if the given energy can be the maximum amount of energy
	 * 
	 * @param energy The given maximum amount of energy
	 * 
	 * @return Energy must be positive and may not exceed the robot's absolute maximum amount of energy.
	 * 		   | energy > 0 && energy < this.getMaxEnergy()
	 */
	public boolean canHaveAsCurrentMaxEnergy(Energy energy) {
		
		return energy.getValueIn(Energy.Unit.WS) > 0;
	}
	
	/**
	 * Checks if the given orientation is a valid orientation
	 * 
	 * @param orientation The given orientation
	 * @return The orientation must be different from null
	 * 		   | orientation != null
	 */
	public boolean canHaveAsOrientation(Orientation orientation) {
		
		return orientation != null;
	}
	
	/**
	 * Return this robot's energy.
	 */
	@Basic @Override
	public Energy getEnergy() {
	
		return energy;
	}

	/**
	 * Set the robot's energy to the given Energy
	 * 
	 * @pre The energy must be valid
	 * 		| canHaveAsEnergy(newEnergy)
	 * 
	 * @param newEnergy The new amount of energy for the robot
	 * 
	 * @post The robot's energy will be newEnergy
	 * 		 | new.getEnergy().getValueIn(Energy.Unit.WS) = newEnergy
	 */
	@Raw @Basic
	public void setEnergy(Energy newEnergy) {
		energy = newEnergy;
	}
	
	/**
	 * Add <code>energyAmount</code> (expressed in watt-second) to <code>robot</code>. If <code>energyAmount</code> is negative or if
	 * adding <code>energyAmount</code> would cause the robot to exceed its maximum energy level, do not modify the state of the robot.
	 * 
	 * @pre energyAmount must be positive
	 * 		| energyAmount >= 0
	 * @pre The sum of the robot's current energy and energyAmount must be inferior to the max Energy
	 * 		| this.getEnergy().getValueIn(Energy.Unit.WS).getValueIn(Energy.Unit.WS) + energyAmount <= Robot.getMaxEnergy()
	 * 
	 * @param energyAmount The Energy that will be added to the current Energy
	 * 
	 * @effect Set the robot's energy to the sum of it's current energy and the energy to add
	 * 		   | this.setEnergy(this.getEnergy().getValueIn(Energy.Unit.WS).getValueIn(Energy.Unit.WS) + energyAmount)
	 */
	@Override
	public void recharge(Energy energyAmount) { //nominally
		
		energy = new Energy(energy.getValueIn(Energy.Unit.WS) + energyAmount.getValueIn(Energy.Unit.WS));
	}

	/**
	 * Return the maximum Energy this robot currently have.
	 * @return The maximum Energy this robot currently have.
	 */
	@Basic @Raw
	public Energy getCurrentMaxEnergy() {
		
		return currentMaxEnergy;
	}

	/**
	 * Set the current maximum Energy to a new Energy.
	 * @param newEnergy The new maximum Energy.
	 * @pre newEnergy must be valid
	 * 		| canHaveAsCurrentMaxEnergy(newEnergy)
	 * @post The new maximum amount of energy will be newEnergy
	 * 		 | (new this).getCurrentMaxEnergy() == newEnergy
	 */
	@Raw @Basic @Model
	private void setCurrentMaxEnergy(Energy newEnergy) {
		currentMaxEnergy = newEnergy;
	}
	
	/**
	 * Return the maximum energy amount this robot may possess.
	 * @return The maximum energy amount this robot may possess.
	 */
	@Basic @Immutable
	public Energy getMaxEnergy() {
		return MAX_ENERGY;
	}

	/**
	 * Returns the ratio of the robot's current energy and current maximum energy.
	 * @return The ratio of the robot's current energy and current maximum energy.
	 * 		   | getEnergy().getValueIn(Energy.Unit.WS)/getCurrentMaxEnergy().getValueIn(Energy.Unit.WS)
	 */
	public double getEnergyFraction() {
		return getEnergy().getValueIn(Energy.Unit.WS)/getCurrentMaxEnergy().getValueIn(Energy.Unit.WS);
	}

	/**
	 * Get the Energy needed to move, if the robot has no load
	 * 
	 * @return the Energy needed to move, if the robot has no load
	 */
	@Basic @Immutable
	public static final Energy getEnergyRequiredToMove() {
		
		return MOVE_PRICE;
	}
	
	/**
	 * Get the Energy needed to turn
	 * 
	 * @return the Energy needed to turn
	 */
	@Basic @Immutable
	public static final Energy getEnergyRequiredToTurn() {
		
		return TURN_PRICE;
	}
	
	/**
	 * Get the Energy needed to shoot
	 * 
	 * @return the Energy needed to shoot
	 */
	@Basic @Immutable
	public static final Energy getEnergyRequiredToShoot() {
		
		return SHOOT_PRICE;
	}
	
	/**
	 * Returns the Energy the robot's maximum Energy is decreased if it is shot.
	 * @return The Energy the robot's maximum Energy is decreased if it is shot.
	 */
	@Basic @Immutable
	public static final Energy getShotDamage() {
		return SHOT_DAMAGE;
	}
	
	/**
	 * Get the factor that determines the extra Energy needed to move if the robot has a load greater than zero
	 * 
	 * @return the factor that determines the extra amount of energy needed to move if the robot has a load greater than zero
	 */
	@Basic @Immutable
	public static final double getLoadFactor() {
		
		return LOAD_FACTOR;
	}
	
	/**
	 * Transfer energy from this robot to an other energyholder.
	 * 
	 * @pre The Energy to be transferred must be positive and may not be greater than the robot's current energy.
	 * 		| amount > 0 && amount < this.getEnergy
	 * @param other The other IEngergyholder to transfer the energy to.
	 * @param amount The Energy to be transferred.
	 * 
	 * @effect The robot will lose the Energy and other will have gained it
	 * 		   | setEnergy(this.getEnergy().getValueIn(Energy.Unit.WS) - amount)
	 * 		   | other.recharge(amount)
	 */
	@Override
	public void transferEnergy(IEnergyHolder other, Energy amount) {
	
		setEnergy(new Energy(getEnergy().getValueIn(Energy.Unit.WS) - amount.getValueIn(Energy.Unit.WS)));
		other.recharge(amount);
	
	}

	/**
	 * Repair this robot's current maximum amount of energy by repairAmount
	 * @param repairAmount The amount of energy that will be added to this robot's current maximum amount of energy.
	 * 
	 * @effect The new maximum amount of energy will be the sum of the current maximum energy and repairAmount.
	 * 		 | setCurrentMaxEnergy(new Energy(getCurrentMaxEnergy().getValueIn(Energy.Unit.WS) + repairAmount.getValueIn(Energy.Unit.WS)))
	 */
	public void repair(Energy repairAmount) {
		setCurrentMaxEnergy(new Energy(getCurrentMaxEnergy().getValueIn(Energy.Unit.WS) + repairAmount.getValueIn(Energy.Unit.WS)));
	}

	/**
	 * Shoot in the orientation the robot is facing, terminating a random element from the closest position to the robot that contains elements.
	 * 
	 * @throws IsTerminatedException
	 * 		   A terminated robot can't shoot
	 * 		   | this.isTerminated()
	 * @throws NullPointerException
	 * 		   If the robot is not on a board it can't shoot.
	 * 		   | getBoard() == null
	 * 
	 * @pre The robot must have enough energy to shoot
	 * 		| this.getEnergy().compare(getEnergyRequiredToShoot().getValueIn(Energy.Unit.WS)) > 0
	 * 
	 * @post A random element from the closest position to the robot that contains elements will get shot.
	 * 		 The element that got shot will be removed from the board and terminated.
	 * 		 | let
	 * 		 | 		shotElement
	 * 		 | in
	 * 		 | 		for any element in this.getBoard().getElements(Element.class())
	 * 		 | 			if !(new this.getBoard()).getElements(Element.class()).contains(element)
	 * 		 | 			then element == shotElement
	 * 		 | 		shotElement.isTerminated()
	 */
	public void shoot() throws IsTerminatedException, NullPointerException {
		if(this.isTerminated()) throw new IsTerminatedException("Robot is terminated");
		if(getBoard() == null) throw new NullPointerException();
		
		Element closestElementInDirection = null;
		for(Element el : getBoard().getElements(Element.class)){
			if(this.getPosition().isPlacedAtOrientationOf(el.getPosition(), getOrientation())){
				if(closestElementInDirection!=null){
					if(this.getPosition().getManhattanDistance(el.getPosition())<this.getPosition().getManhattanDistance(closestElementInDirection.getPosition())){
						closestElementInDirection = el;
					}
				} else { 
					closestElementInDirection = el;
				}
			}
		}
		
		if(closestElementInDirection != null){
			Position shootPosition = closestElementInDirection.getPosition();
			ArrayList<Element> shootElements = new ArrayList<Element>();
			for(Element element : getBoard().getElements(shootPosition)) {
				shootElements.add(element);
			}
	
			Random rand = new Random();
			int shot = rand.nextInt(shootElements.size());
	
			shootElements.get(shot).takeShot();
		}
		
		setEnergy(new Energy(getEnergy().getValueIn(Unit.WS) - Robot.getEnergyRequiredToShoot().getValueIn(Unit.WS)));
	}

	/**
	 * Invoked when the robot is shot by a laser. When shot the robot's maximum energy decreases by getShotDamage().
	 * If the robot's maximum energy reaches 0 it is terminated.
	 * 
	 * @post When shot the robot's maximum energy decreases by getShotDamage(). If the robot's maximum energy reaches 0 it is terminated.
	 * 		 | if(getMaxEnergy() < getShotDamage())
	 * 		 | then terminate
	 * 		 | else
	 * 		 | 		setCurrentMaxEnergy(getMaxEnergy()-getShotDamage())
	 * 		 |		if(getEnergy().getValueIn(Energy.Unit.WS) > getMaxEnergy())
	 * 		 |		setEnergy(getMaxEnergy())
	 */
	@Override
	public void takeShot() {
		if(getCurrentMaxEnergy().compareTo(getShotDamage()) <= 0){
			terminate();
		}
		else{
			setCurrentMaxEnergy(new Energy(getCurrentMaxEnergy().getValueIn(Energy.Unit.WS)-getShotDamage().getValueIn(Energy.Unit.WS)));
			if(getEnergy().getValueIn(Energy.Unit.WS) > getCurrentMaxEnergy().getValueIn(Energy.Unit.WS)) setEnergy(getCurrentMaxEnergy());
		}
		
	}

	/**
	 * The robot's current energy
	 */
	private Energy energy;

	/**
	 * The maximal Energy a robot currently has
	 */
	private Energy currentMaxEnergy;

	/**
	 * The greatest maximal Energy a robot may have
	 */
	private final Energy MAX_ENERGY;

	/**
	 * The energy amount a robot needs to use to turn once
	 */
	private final static Energy TURN_PRICE = new Energy(100);

	/**
	 * The minimum Energy a robot needs to use to move once
	 */
	private final static Energy MOVE_PRICE = new Energy(500);

	/**
	 * The Energy a robot needs to use to shoot
	 */
	private final static Energy SHOOT_PRICE = new Energy(1000);

	/**
	 * The Energy the robot's maximum Energy is decreased if it is shot.
	 */
	private final static Energy SHOT_DAMAGE = new Energy(4000);

	/**
	 * The factor that determines how much energy in Ws a robot requires to move once based on the robot's current load
	 */
	private final static double LOAD_FACTOR = 1/200.0;

	/**
	 *  Gives the Energy required to reach a given position from the current position of the robot. This involves moving and turning.
	 * 
	 * @param position The position to calculate the required energy for
	 * 
	 * @throws IllegalPositionException
	 * 		   the given position is outside the board
	 * 		   | !getBoard().isValidPosition(position)
	 * 
	 * @throws IsTerminatedException
	 * 		   The robot is terminated
	 * 		   | this.isTerminated()
	 * 
	 * @return -1 if the board is null
	 * 		   | if this.getBoard() == null
	 * 		   | then result == -1
	 * 
	 * @return -1 if the robot is blocked by obstacles
	 * 		   | if manhattanCostToReach(position.getX(), position.getY(), this.getOrientation().intValue()) <= this.getEnergy().getValueIn(Energy.Unit.WS)
	 * 		   | then result == -1
	 * 
	 * @return -2 if the robot doesn't have enough energy to reach the given position
	 * 		   | if !getConverter().getRobotRange().contains(position)
	 * 		   | then result == -2
	 * 
	 * @return The energy needed to reach the given position, or :
	 * 		   -1 if the board is null,
	 * 		   -1 if the robot is blocked by obstacles
	 * 		   -2 if the robot doesn't have enough energy to reach the given position
	 * 		   | if this.getBoard() == null
	 * 		   | then result == -1
	 *  	   | else if manhattanCostToReach(position.getX(), position.getY(), this.getOrientation().intValue()) <= this.getEnergy().getValueIn(Energy.Unit.WS)
	 * 		   | 	then result == -1
	 * 		   | 	else if !getConverter().getRobotRange().contains(position)
	 * 		   | 		then result == -2
	 * 		   | 	else result == getConverter().getEnergyToReach(position)
	 */
	public double getMinimalCostToReach(Position position) throws IllegalPositionException, IsTerminatedException {
		
		if(this.isTerminated()) throw new IsTerminatedException("Cannot perform this action on a terminated Robot");
		if(this.getBoard() == null) return -1;
		if(!getBoard().isValidPosition(position)) throw new IllegalPositionException("Not valid position for this board ");
		if(!getConverter().getRobotRange().contains(position)) {
			if(manhattanCostToReach(position.getX(), position.getY(), this.getOrientation().intValue()) <= this.getEnergy().getValueIn(Energy.Unit.WS)){
				return -1; //cannot reach because of obstacles
			}
			//This was -2 in the previous version of the assignment
			return -1; //cannot reach because of lack of energy
		}
		return getConverter().getEnergyToReach(position);
	}
	
	/**
	 * Gives the Energy required to reach a given position from the current position of the robot. This involves moving and turning.
	 * 
	 * @throws IllegalPositionException
	 * 		   The given x- or y-coordinate is not valid
	 * 		   |  !getBoard().isValidPosition(new Position(x, y))
	 * 
	 * @param x The x-coordinate of the position to reach
	 * @param y The y-coordinate of the position to reach
	 * @param ori The integer value of the robot's orientation if it would move to the given coordinates
	 * 
	 * @return The Energy needed. This is the amount of turns needed multiplied by the cost of a turn 
	 * 		   added to the amount of moves needed multiplied by the cost of a move.
	 * 		   | let 
	 * 		   |	orientationtoMove1
	 * 		   |	orientationToMove2
	 * 		   |	turnsNeeded
	 * 		   | in
	 * 		   |	if(this.getPosition().getX() != x) 
	 * 		   |	then if(this.getPosition().getX() < x) 
	 * 		   |			then orientationToMove1 = 1
	 * 		   |			orientationToMove2 = 1 //For the case this.y == y
	 * 		   |
	 * 		   |		if(this.getPosition().getX() > x) 
	 * 		   |			then orientationToMove1 = 3
	 * 		   |			orientationToMove2 = 3 //For the case this.y == y
	 * 		   |
	 * 		   |	if(this.getPosition().getY() != y) 
	 * 		   |	then if(this.getPosition().getY() < y) 
	 * 		   |		 then orientationToMove2 = 2
	 * 		   |	 	 if(this.getPosition().getX() == x) 
	 * 		   |	  	 then orientationToMove1 = 2
	 * 		   |
	 * 		   |		 if(this.getPosition().getY() > y) 
	 * 		   |		 then orientationToMove2 = 0
	 * 		   |		 
	 * 		   |		 if(this.getPosition().getX() == x) 
	 * 		   |		 then orientationToMove1 = 0
	 * 		   |
	 * 		   |	if( !(this.getPosition().getX() == x && this.getPosition().getY() == y))  //only turn if required to move
	 * 		   |
	 * 		   |	then if(this.getPosition().getX() != x && this.getPosition().getY() != y) turnsNeeded++
	 * 		   |
	 * 		   |		 if(ori != orientationToMove1 && ori != orientationToMove2) 
	 * 		   |		 then turnsNeeded++
	 * 		   |			  if((this.getPosition().getX() == x || this.getPosition().getY() == y) && (ori == 0 && orientationToMove1 == 2 ||
	 * 		   |												  ori == 1 && orientationToMove1 == 3 ||
	 * 		   |											      ori == 2 && orientationToMove1 == 0 ||
	 * 		   |												  ori == 3 && orientationToMove1 == 1)) 
	 * 		   |			  then turnsNeeded++
	 * 		   |	
	 * 		   | result == (Math.abs(this.getPosition().getX() - x) + Math.abs(this.getPosition().getY() - y))*(this.getEnergyrequiredToMove()+getLoadFactor()*getLoad())
	 * 					  + turnsNeeded*this.getEnergyRequiredToTurn()
	 */
	@Model
	private double manhattanCostToReach(long x, long y, int ori) throws IllegalPositionException {
		
		long movesNeeded = Math.abs(this.getPosition().getX() - x) + Math.abs(this.getPosition().getY() - y);
		int turnsNeeded1 = 0;
		int orientationToMove1 = 0;
		int orientationToMove2 = 0;
		if(this.getPosition().getX() != x) {
			if(this.getPosition().getX() < x) {
				orientationToMove1 = 1;
				orientationToMove2 = 1; //For the case this.getPosition().getY() == y
			}
			if(this.getPosition().getX() > x) {
				orientationToMove1 = 3;
				orientationToMove2 = 3; //For the case this.getPosition().getY() == y
			}
		}
		if(this.getPosition().getY() != y) {
			if(this.getPosition().getY() < y) {
				orientationToMove2 = 2;
				if(this.getPosition().getX() == x) orientationToMove1 = 2;
			}
			if(this.getPosition().getY() > y) {
				orientationToMove2 = 0;
				if(this.getPosition().getX() == x) orientationToMove1 = 0;
			}
		}
		if( !(this.getPosition().getX() == x && this.getPosition().getY() == y)) { //only turn if required to move
			
			if(this.getPosition().getX() != x && this.getPosition().getY() != y) turnsNeeded1++;
			
			if(ori != orientationToMove1 && ori != orientationToMove2) {
				turnsNeeded1++;
				if((this.getPosition().getX() == x || this.getPosition().getY() == y) && (ori == 0 && orientationToMove1 == 2 ||
													ori == 1 && orientationToMove1 == 3 ||
													ori == 2 && orientationToMove1 == 0 ||
													ori == 3 && orientationToMove1 == 1)) turnsNeeded1++;
			}
		}
		int turnsNeeded = turnsNeeded1;
		
		return movesNeeded*(MOVE_PRICE.getValueIn(Energy.Unit.WS)+getLoadFactor()*getLoad()) + turnsNeeded*TURN_PRICE.getValueIn(Energy.Unit.WS);
	}
	
	/**
	 * Move the robot forwards.
	 * 
	 * @throws IllegalPositionException
	 * @throws IsTerminatedException
	 * 		   cannot move a terminated robot
	 * 		   | isTerminated()
	 * @throws NullPointerException
	 * 		   The robot is not on a board
	 * 		   | getBoard() == null
	 * 
	 * @effect Set the position of the robot to it's neighbor.
	 * 		   |  setPosition(getPosition().getNeighbour(getOrientation()))
	 * 
	 * @effect Reduce the Energy by the energy required to move
	 * 		   | setEnergy(getEnergy().getValueIn(Energy.Unit.WS).getValueIn(Energy.Unit.WS) - (getEnergyRequiredToMove() + getLoad()*getLoadFactor()))
	 */
	public void move() throws IllegalPositionException, IsTerminatedException, NullPointerException { //defensively
		
		if(isTerminated()) throw new IsTerminatedException("Can't move a terminated robot");
		if(getBoard() == null) throw new NullPointerException("Robot has no board, so he can't move");
		
		setPosition(getPosition().getNeighbour(getOrientation()));
		
		setEnergy(new Energy(getEnergy().getValueIn(Energy.Unit.WS) - (getEnergyRequiredToMove().getValueIn(Energy.Unit.WS) + getLoad()*getLoadFactor())));
	}

	/**
	 * Move a robot next to another robot, provided they have enough energy to do so. If not, move as close as possible to each other.
	 * Do this while using a minimal amount of energy.
	 * 
	 * @post The two robots move as close as possible to each other while using a minimal amount of energy :
	 * 		 Of all the positions the robots can reach with their current amount of energy they will be at those positions
	 * 		 that minimize the distance between them.
	 * 		 If multiple couples of positions with this minimal distance exist the positions that require the
	 * 		 smallest amount of energy (in total for both robots) are used.
	 * 
	 * 		   | let
	 *  	   |	reachable1[]
	 *  	   |	reachable2[]
	 *  	   | in
	 * 		   | 	for each x in 0..this.getBoard().getWidth()
	 *  	   |		for each y in 0..this.getBoard().getHeight()
	 *  	   |			if(this.getMinimalCostToReach(new Position(x, y))<=this.getEnergy().getValueIn(Energy.Unit.WS).getValueIn(Energy.Unit.WS))
	 *  	   |			then reachable1.contains({x,y})
	 *  	   |
	 *  	   |			if(this.getMinimalCostToReach(new Position(x, y))<=robot.getEnergy().getValueIn(Energy.Unit.WS).getValueIn(Energy.Unit.WS))
	 *  	   |			then reachable2.contains({x,y})
	 *  	   |
	 *  	   |	for each reach1 in reachable1
	 *  	   |		for each reach2 in reachable2
	 *  	   |			Math.abs(new.getX() - (new other).getPosition().getX()) + Math.abs(new.getPosition().getY() - (new other).getPosition().getY)
	 *  	   |			<= Math.abs(reach1[0] - reach1[1]) + Math.abs(reach2[0] - reach2[1])
	 *  	   |
	 *  	   |			if(Math.abs(reach1[0] - reach1[1]) + Math.abs(reach2[0] - reach2[1])
	 *  	   |			   == Math.abs(new.getPosition().getX() - (new other).getPosition().getX()) + Math.abs(new.getPosition().getY() - (new other).getPosition().getY)
	 *  	   |			then this.getEnergyRequiredToReach(new.getPosition().getX(),new.getPosition().getY()) + other.getEnergyRequiredToReach((new other).getPosition().getX(),(new other).getPosition().getY)
	 *  	   |				 <= this.getMinimalCostToReach(new Position(reach1[0],reach1[1])) + other.getMinimalCostToReach(new Position(reach2[0],reach2[1]))
	 *  	
	 * 			
	 * @param other The robot to move next to.
	 * 
	 * @throws IsTerminatedException
	 * 		   A terminated robot cannot move
	 * 		   | this.isTerminated() || other.isTerminated()
	 * 
	 * @throws NullPointerException
	 * 		   This board must not be null
	 * 		   | this.getBoard() == null
	 * 
	 * @throws IllegalPositionException
	 * 		   The two robots must be on the same board
	 * 		   | this.getBoard() != other.getBoard()
	 */
	public void moveNextTo(Robot other) throws IsTerminatedException, NullPointerException, IllegalPositionException {
		
		if(this.isTerminated() || other.isTerminated()) throw new IsTerminatedException("One of the robots is terminated");
		if(this.getBoard() == null) throw new NullPointerException();
		if(this.getBoard() != other.getBoard()) throw new IllegalPositionException("Different boards, moving next to not possible.");
		
		Range rangeA = getConverter().getRobotRange();
		Range rangeB = other.getConverter().getRobotRange();
		
		for(Orientation ori : Orientation.values()) {
			try {
				if(rangeA.contains(other.getPosition().getNeighbour(ori))) {
					rangeA.addPosition(other.getPosition());
				}
			}
			catch(IllegalPositionException e) {
				
			}
			try {
				if(rangeB.contains(getPosition().getNeighbour(ori))) {
					rangeB.addPosition(getPosition());
				}
			}
			catch(IllegalPositionException e) {
				
			}
			
		}
		
		List<Set<Position>> pairs = Range.posWithShortestDistanceBetween(rangeA, rangeB);
		
		Position[] minPos = new Position[2];
		double minTotalEnergy = Double.MAX_VALUE;
		for(Set<Position> positions : pairs) {
			Iterator<Position> it = positions.iterator();
			Position positionA = it.next();
			Position positionB = it.next();
			double noSwapEnergy;
			double swapEnergy;
			try {
				double minA = this.getMinimalCostToReach(positionA);
				double minB = other.getMinimalCostToReach(positionB);
				noSwapEnergy = minA+minB;
				if(minA < 0 || minB < 0) {
					noSwapEnergy = Double.MAX_VALUE;
				}
			}
			catch(Exception e) {
				noSwapEnergy = Double.MAX_VALUE;
			}
			try {
				double minA = this.getMinimalCostToReach(positionB);
				double minB = other.getMinimalCostToReach(positionA);
				swapEnergy = minA+minB;
				if(minA < 0 || minB < 0) {
					swapEnergy = Double.MAX_VALUE;
				}
			}
			catch(Exception e) {
				swapEnergy = Double.MAX_VALUE;
			}
			if(noSwapEnergy >= swapEnergy){
				Position swap;
				swap = new Position(positionA.getX(), positionA.getY());
				positionA =  new Position(positionB.getX(), positionB.getY());
				positionB = swap;
			}
			double currentTotalEnergy = this.getMinimalCostToReach(positionA)+other.getMinimalCostToReach(positionB);
			if(currentTotalEnergy < minTotalEnergy){
				minPos[0] = positionA;
				minPos[1] = positionB;
				minTotalEnergy = currentTotalEnergy;
			}
		}
		this.setEnergy(new Energy(getEnergy().getValueIn(Energy.Unit.WS) - getMinimalCostToReach(minPos[0])));
		this.setOrientation(converter.getPostMoveOrientation(minPos[0]));
		this.setPosition(minPos[0]);
		other.setEnergy(new Energy(other.getEnergy().getValueIn(Energy.Unit.WS) - other.getMinimalCostToReach(minPos[1])));
		other.setOrientation(other.getConverter().getPostMoveOrientation(minPos[1]));
		other.setPosition(minPos[1]);
	}

	/**
	 * Checks if the robot could share a position on a board with the given Element.
	 * 
	 * @param other The element to check.
	 * 
	 * @return Robots can only share positions with Items.
	 * 		   | Item.class.isInstance(other)
	 */
	@Override
	public boolean canSharePositionWith(Element other) {
		boolean result = false;
		
		if(Item.class.isInstance(other)) { //can only share position with items
			result = true;
		}
		
		return result;
	}

	/**
	 * Get the robot's BoardGraphConverter.
	 * 
	 * @return The robot's BoardGraphConverter.
	 */
	@Basic
	public BoardGraphConverter getConverter() {
		return converter;
	}

	/**
	 * The BoardGraphConverter of this robot. This converter is necessary to find the range of a robot,
	 * and the minimal amount of energy required to move in this range.
	 */
	private BoardGraphConverter converter = new BoardGraphConverter(this);

	/**
	 * Set the robot's position to the given position
	 * 
	 * @pre The robot must be on a board
	 * 		| getBoard() != null
	 * 
	 * @param newPosition The robot's new position
	 * 
	 * @throws IllegalPositionException
	 * 		   The position is not valid
	 * 		   | !getBoard().isValidPosition(newPosition)
	 * 
	 * @effect Set the position to the new position
	 * 		   | setBoardPosition(getBoard(), newPosition)
	 */
	@Model
	private void setPosition(Position newPosition) throws IllegalPositionException {
		
		if(!getBoard().isValidPosition(newPosition)) throw new IllegalPositionException("The new position is not valid for this board");
	
		if(!getPosition().equals(newPosition)) {
			try {
				this.setBoardPosition(getBoard(), newPosition);
			} catch (IllegalArgumentException e) {
				throw new IllegalPositionException("Could not move here");
			}
		}
		
		
	}

	/**
	 * Pick up an item from the board and add it to the robot's possessions.
	 * An item can only be picked up if the robot and the item are on the same position on the same board.
	 * 
	 * @param item The item to be picked up.
	 * @throws IllegalArgumentException
	 * 		   It is not possible to pick up an item with a board and position different from the robot's.
	 * 		   | getBoard() != item.getBoard() || !getPosition().equals(item.getPosition())
	 * @throws NullPointerException
	 * 		   If an item is null or the robot is not on a board.
	 * 		   | item == null || getBoard() == null
	 * @throws IsTerminatedException
	 * 		   Terminated robots can not pick up items.
	 * 		   | this.isTerminated()
	 * 
	 * @effect The item will be removed from the board and added to this robot's items.
	 * 		   | item.getBoard().removeElement(item)
	 * 		   | this.addItem(item)
	 */
	public void pickUp(Item item) throws IllegalArgumentException, NullPointerException, IsTerminatedException {
		
		if(this.isTerminated()) throw new IsTerminatedException("Cannot perform this action on a terminated Robot");
		if(item == null || getBoard() == null)
			throw new NullPointerException();
		if(getBoard() != item.getBoard() || !getPosition().equals(item.getPosition())) 
			throw new IllegalArgumentException("The board or the position is different");
		
		item.getBoard().removeElement(item);
	
		addItem(item);
	}

	/**
	 * Use an item in the robot's possession
	 * 
	 * @param item The item to use
	 * 
	 * @throws IsTerminatedException
	 * 		   A terminated robot cannot use an item
	 * 		   | this.isTerminated()
	 * 
	 * @effect Use the item
	 * 		   | item.use(this)
	 */
	public void use(Item item) throws IsTerminatedException{
		
		if(this.isTerminated()) throw new IsTerminatedException("Terminated Robots cannot use items");
		if(getItems().contains(item)) {
			if(item.isTerminated()) {
				items.remove(item);
			}
			else {
				item.use(this);
				if(item.isTerminated()) {
					items.remove(item);
				}
			}
		}
	}
	
	/**
	 * Drop an item in the robot's possession onto the board.
	 * @param item The item to drop.
	 * @throws IsTerminatedException
	 * 		  | this.isTerminated()
	 * @throws NullPointerException
	 * 		  | this.getBoard() == null
	 * 
	 * @post The item will be on the board, on the robot's current position. The robot will not possess the item any more.
	 * 		 | this.getBoard().getElements(Item.class).contains(item)
	 * 		 | this.getBoard().getElements(this.getPosition()).contains(item)
	 * 		 | !new.getItems().contains(item)
	 */
	public void drop(Item item) throws IsTerminatedException, NullPointerException{
		
		if(isTerminated()) throw new IsTerminatedException("Cannot drop item if terminated");
		
		//only if item in possessions
		if(getItems().contains(item)) {
			items.remove(item);
			if(getBoard() != null) item.setBoardPosition(getBoard(),getPosition());
		}
		
	}

	/**
	 * Gives a set with all the robot's items
	 * 
	 * @return a set with all the robot's items
	 * 
	 */
	@Basic
	public java.util.Set<Item> getItems() {
		
		return new HashSet<Item>(items);
	}

	/**
	 * Returns the number of items this robot is carrying.
	 * @return The number of items this robot is carrying.
	 * 		   | this.getItems().size()
	 */
	public int getNbItems() {
		return getItems().size();
	}

	/**
	 * Returns the i-th heaviest item this robot is carrying. (The heaviest item being 1, the second heaviest 2, ...)
	 * @param i The index in the item list sorted according to weight.
	 * @throws IndexOutOfBoundsException
	 * 		   If the number given is greater than the number of items the robot is carrying.
	 * 		   | i>getNbItems()
	 * @return The i-th heaviest item this robot is carrying.
	 * 		   | let
	 * 		   | 	lighterItems
	 * 		   | 	sameWeightItems
	 * 		   | 	heavierItems
	 * 		   | in
	 * 		   | 	lighterItems = #{ item : this.getItems() | item.getWeight() < result.getWeight()}
	 * 		   | 	sameWeightItems = #{ item : this.getItems() | item.getWeight() == result.getWeight() && !item.equals(result)}
	 * 		   | 	heavierItems = #{ item : this.getItems() | item.getWeight() > result.getWeight()}
	 * 		   | 	lighterItems + sameWeightItems + heavierItems + 1 == getNbItems()
	 * 		   | 	for some x in 0..sameWeightItems
	 * 		   | 		i == lighterItems + x && i == getNbItems()-heavierItems-(sameWeightItems-x)
	 */
	public Item getIHeaviestItem(int i) throws IndexOutOfBoundsException {
		return items.get(i-1);
	}

	/**
	 * Add an item to the robot's item list. The item is placed in the sorted list according to it's weight.
	 * 
	 * @param item The item to add.
	 * @throws NullPointerException
	 * 		   Null can not be added.
	 * 		   | item == null
	 * @post The item will be in the robot's item list.
	 * 		 | this.getItems().contains(item)
	 */
	@Model
	private void addItem(Item item) throws NullPointerException {
		if(item == null)
			throw new NullPointerException();
		boolean added = false;
		for(Item it : items) {
			if(it.getWeight() < item.getWeight()) {
				items.add(items.indexOf(it), item);
				added = true;
				break; //prevent adding the same item more than once
			}
		}
		if(!added) items.add(item);
	}

	/**
	 * The list of items this robot possess.
	 */
	private List<Item> items;

	/**
	 * Get the total load of the robot (the sum of weight of all items the robot possesses)
	 * 
	 * @return the total load of the robot (the sum of weight of all items the robot possesses)
	 * 		   | result == sum (for each item in getItems() item.getWeight())
	 */
	public int getLoad() {
		
		int load = 0;
		for(Item item : items) {
			load += item.getWeight();
		}
		
		return load;
	}
	
	/**
	 * Transfer all this robot's items to the given robot, provided that they are neighbours.
	 * 
	 * @param robot The robot that will receive all this robot's items.
	 * 
	 * @throws IllegalArgumentException Throw this exception if the robot's are on different boards or if they are not neighbours.
	 * 		   | let 
	 * 		   | 	neighbours = false
	 * 		   | in
	 * 		   | 	for each orientation in Orientation.values()
	 * 		   |		if 	this.getPosition().getNeighbour(orientation).equals(robot.getPosition())
	 * 		   |		then neighbours = true
	 * 		   |	robot.getBoard() == null || this.getBoard() != robot.getBoard() || !neighbours
	 * 
	 * @post This robot will give all its items
	 * 		 | (new this).getNbItems() == 0
	 * @post The given robot will possess this robot's items.
	 * 		 | (new robot).getNbItems() == robot.getNbItems() + this.getNbItems()
	 * 		 | for each item in this.getItems()
	 * 		 |		(new robot).getItems().contains(item.clone())
	 * 		 | for each item in robot.getItems()
	 * 		 | 		(new robot).getItems().contains(item)
	 */
	public void transferItemsToRobot(Robot robot) throws IllegalArgumentException{
		if(robot.getBoard() == null || this.getBoard() != robot.getBoard()) throw new IllegalArgumentException();
		boolean neighbours = false;
		for(Orientation orientation : Orientation.values()){
			try {
				if(this.getPosition().getNeighbour(orientation).equals(robot.getPosition())){
					neighbours = true;
				}
			} catch (IllegalPositionException e) {}
		}
		if(!neighbours) throw new IllegalArgumentException("The robots are not standing next to each other.");
		Set<Item> items = this.getItems();
		Set<Item> clones = new HashSet<Item>();
		for(Item item : items){
			clones.add(item.clone());
			this.drop(item);
			item.terminate();
		}
		for(Item clone : clones){
			clone.setBoardPosition(robot.getBoard(), robot.getPosition());
			robot.pickUp(clone);
		}
		
		
	}

	/**
	 * Returns the program the robot is currently storing. Returns null if the robot isn't storing a program.
	 */
	@Basic
	public Command getProgram() {
		return program;
	}

	/**
	 * Set the robot's program to the given program.
	 */
	@Basic
	public void loadProgram(String name) {
		
		this.program = ScriptReader.commandFromFileForRobot(name, this);
	}

	/**
	 * Executes one step of the robot's program.
	 * 
	 * @effect Get the command that is the robot's program and execute one Step on it.
	 * 		   | this.getProgram().step()
	 */
	public void stepProgram() {
		if(getProgram() != null) getProgram().step();
		
	}

	/**
	 * The robot's program.
	 */
	private Command program;

	/**
	 * Clone the robot.
	 * 
	 * @return The new robot will have all attributes of this robot, except it's board and position, because it's impossible for a robot
	 * 		   to share it's position with another robot. The clone's possessions will be clones of this robot's possessions.
	 * 		   | clone.getOrientation() == this.getOrientation() && clone.getEnergy().getValueIn(Energy.Unit.WS) == this.getEnergy().getValueIn(Energy.Unit.WS) && clone.getMaxEnergy() == this.getMaxEnergy()
	 * 		   | if this.getItems().contains(item)
	 * 		   | then clone.getItems().contains(item.clone())
	 * 		   | else !clone.getItems().contains(item.clone())
	 */
	@Override
	public Robot clone() {
		
		if(this.isTerminated()) throw new IsTerminatedException("Cannot clone terminated Robots");
		Robot clone = new Robot(null, null, getOrientation(), getEnergy().getValueIn(Energy.Unit.WS), getCurrentMaxEnergy().getValueIn(Energy.Unit.WS));
		for(Item item : items) {
			clone.addItem((Item) item.clone());
		}
		
		return clone;
	}
	
	/**
	 * Terminate this robot.
	 */
	@Override
	public void terminate() {

		while(!items.isEmpty()) {
			Item i = items.get(0);
			items.remove(i);
			i.terminate();
		}
		
		super.terminate();
	}
	
	/**
	 * Generate a String representation of this object.
	 */
	@Override
	public String toString() {
		if(this.isTerminated()) return "A terminated robot";
		String result = "Robot" + getPosition() + ", " + getEnergy().getValueIn(Energy.Unit.WS) + "/" +
						getCurrentMaxEnergy().getValueIn(Energy.Unit.WS) + " ws, " + getLoad() + "g [";
		for(Item item : getItems()) {
			result += "\n" + item;
		}
		
		return result + " ]";
	}

}
